#!/usr/bin/env python
import roslib; roslib.load_manifest('EventPlanner')

import rospy
import actionlib
import math
import tf
import copy

from math import *

from nav_msgs.msg import Odometry
from geometry_msgs.msg import Quaternion, Point, PoseStamped

from tf.transformations import euler_from_quaternion
from pr2_controllers_msgs.msg import PointHeadAction, PointHeadGoal
from EventPlanner.msg import MoveBaseServerGoal, MoveHeadServerGoal
from move_base_msgs.msg import MoveBaseGoal, MoveBaseAction, MoveBaseActionFeedback

class MoveBaseServer:
	def __init__(self):
		rospy.logwarn("Move Base Server Initializing...")

		self.running = False
		self.curOdom = None
		self.lastOdom = None
		self.headGoal = None
		self.baseGoal = None

		statedInitializeInfo = False

		self.minDistance = 0.0
		self.moveAroundGoal = False
		self.movingAroundGoal = False
		self.lookAtGoal = False
		self.updateOrientation = False
		self.allowOverride = True

		self.tl = tf.TransformListener()

		self.pubHead = rospy.Publisher("moveHead",MoveHeadServerGoal);

		baseFeedBack = rospy.Subscriber("move_base/feedback",MoveBaseActionFeedback, self.feedbackCallback)
		#baseStatus = rospy.Subscriber("/base_odometry/odom",Odometry, self.baseOdometry)
		goalSub    = rospy.Subscriber("moveBase", MoveBaseServerGoal, self.moveBase)

		self.baseClient   = actionlib.SimpleActionClient('move_base', MoveBaseAction)
		while self.baseClient.wait_for_server(rospy.Duration(1.0)):
			if not statedInitializeInfo:
				statedInitializeInfo = True
				rospy.loginfo("Waiting for Move Base Client To Initialize...")
			rospy.sleep(.5)

		rospy.logwarn("Initialization of Move Base Server Finished")

	def moveBase(self, newGoal):
		if self.running:
			if ( (newGoal.overridePrevious is None) or (not newGoal.overridePrevious)):
				return
			elif not self.allowOverride:
				return
			else:
				self.baseClient.cancel_goal()

		self.running = True
		self.headGoal = None

		if newGoal.allowOverride is None:
			self.allowOverride = True	# Default to True
		else:
			self.allowOverride = newGoal.allowOverride

		if newGoal.minDistanceFromGoal is None:
			self.minDistance = 0.0
		else:
			self.minDistance = newGoal.minDistanceFromGoal

		if newGoal.lookAtGoal is None:
			self.lookAtGoal = False
		else:
			self.lookAtGoal = newGoal.lookAtGoal

		if newGoal.moveAroundGoal is None:
			self.moveAroundGoal = False
		else:
			self.moveAroundGoal = newGoal.moveAroundGoal
		
		if newGoal.updateOrientation is None:
			self.updateOrientation = False
		else:
			self.updateOrientation = newGoal.updateOrientation

		self.movingAroundGoal = False
		self.moveBaseToGoal(newGoal.goal)

	def moveBaseToGoal(self, goal):
		if self.lookAtGoal:
			#rospy.logwarn("lookAtGoal")
			if self.headGoal is None:
				self.headGoal = MoveHeadServerGoal()
				self.headGoal.goal.target.header = goal.target_pose.header
				self.headGoal.goal.target.point = copy.deepcopy(goal.target_pose.pose.position)
				self.headGoal.overridePrevious = True
			self.pubHead.publish(self.headGoal)

		self.baseGoal = goal
		self.baseClient.send_goal(goal)
		self.running = False

	# Gives Angle about the Z-Axis
	def getTheta(self,orientation):
		angle = euler_from_quaternion([orientation.x, orientation.y, orientation.z, orientation.w])
		return (angle[2] + pi)

	def dist(self,pos1,pos2):
		# Only use the distance in x and y direction.  
		return sqrt(pow((pos1.x - pos2.x),2) + pow((pos1.y - pos2.y),2));

	def shouldUpdateHead(self, deltaTheta, deltaPosition):
		# Check Orientation Difference
		deltaTime = rospy.Time.now() - self.headGoal.goal.target.header.stamp
		if deltaTime.to_sec() < .5:
			return False

		if deltaTheta > pi / 360:
			return True
		if deltaPosition > 0.1:
			return True
		return False
	
	def shouldUpdateBase(self, deltaTheta, deltaPosition):
		if self.lastOdom.feedback.base_position.pose.orientation is self.curOdom.feedback.base_position.pose.orientation:
			return False

		deltaTime = rospy.Time.now() - self.baseGoal.target_pose.header.stamp
		if deltaTime.to_sec() < .5:
			return False

		if deltaTheta > 2 * pi / 360:
			return True
		if deltaPosition > 0.1:
			return True
		return False

	def feedbackCallback(self,currentOdom):
		self.curOdom = currentOdom
		if not self.updateOrientation:
			return

		# Sanity Checks
		if self.lastOdom is None:
			self.lastOdom = self.curOdom
			return

		if self.lastOdom is self.curOdom:
			return
			
		lastTheta = self.getTheta(self.lastOdom.feedback.base_position.pose.orientation)
		curTheta = self.getTheta(self.curOdom.feedback.base_position.pose.orientation)
		deltaTheta = fabs(lastTheta - curTheta)
		distance = self.dist(self.lastOdom.feedback.base_position.pose.position,self.curOdom.feedback.base_position.pose.position) 
		distanceToGoal = self.dist(self.curOdom.feedback.base_position.pose.position, self.baseGoal.target_pose.pose.position)

		if self.movingAroundGoal:
			if self.shouldUpdateHead(deltaTheta,distance):
				self.updateHead()
			return

		if distanceToGoal <= self.minDistance:
			rospy.logwarn("Close enough")
			self.baseClient.cancel_goal()
			if self.moveAroundGoal:
				rospy.logwarn("Moving Around")
				self.moveAround()
			else:
				rospy.logwarn("Not Moving Around")
			return

		# Update the base less frequently than updateHead
		# But when base is updated, the head is updated as well
		if self.shouldUpdateBase(deltaTheta,distance):
			self.lastOdom = self.curOdom
			self.updateGoalOrientation()
			return
		if self.shouldUpdateHead(deltaTheta, distance):
			self.updateHead()
		
	def moveAround(self):
		self.movingAroundGoal = True
		rospy.logwarn("Moving Around")

		# Compute distance to travel
		dx = self.baseGoal.target_pose.pose.position.x - self.curOdom.feedback.base_position.pose.position.x;
		dy = self.baseGoal.target_pose.pose.position.y - self.curOdom.feedback.base_position.pose.position.y;
		newX = math.cos(pi/4)*dx - math.sin(pi/4)*dy;
		newY = math.sin(pi/4)*dx + math.cos(pi/4)*dy;

		newX += self.curOdom.feedback.base_position.pose.position.x;
		newY += self.curOdom.feedback.base_position.pose.position.y;
		curTheta = self.getTheta(self.curOdom.feedback.base_position.pose.orientation)

		# Update the new goal position and orientation
		self.baseGoal.target_pose.pose.position.x = newX;
		self.baseGoal.target_pose.pose.position.y = newY;
		quaternion = tf.transformations.quaternion_about_axis(curTheta+3*pi/4,(0,0,1))
		self.baseGoal.target_pose.pose.orientation = Quaternion(*quaternion);
		self.baseClient.send_goal(self.baseGoal)


	def updateGoalOrientation(self):
		self.baseGoal.target_pose.pose.orientation = self.curOdom.feedback.base_position.pose.orientation
		self.baseGoal.target_pose.header.stamp = rospy.Time.now()
		self.moveBaseToGoal(self.baseGoal)

	def updateHead(self):
		if not self.lookAtGoal or self.headGoal is None:
			return
		self.lastOdom = self.curOdom
		self.headGoal.goal.target.header.stamp = rospy.Time.now()
		self.pubHead.publish(self.headGoal)
		

if __name__=="__main__":
    rospy.init_node("MoveBaseServer")
    server = MoveBaseServer()
    rospy.spin()
